#include "modules/CtrlModule/PassivePlanner/CliffReactionPlanner.h"
#include "lib/WrapFuncs.h"
#include "data_struct/geometry/Point.h"
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"
#include "Communication/InterruptAndRecoveryCom.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
CliffReactionPlanner cliff_reaction_planner;

void CliffReactionPlanner::run(double &linear_vel, double &angular_vel)
{
    BackAndTurn(linear_vel, angular_vel);
}

void CliffReactionPlanner::BackAndTurn(double &linear_vel, double &angular_vel)
{
    // 如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
    if (time_now < 1e-4) time_now = Time::Now();
    std::cout << "BackAndTurn time: " << Time::Now() - time_now << std::endl;
    if (Time::Now() - time_now < 1) // 1s内后退
    {
        linear_vel = -0.25;
        angular_vel = 0;
    }
    else if ((Time::Now() - time_now > 1) && (Time::Now() - time_now < 2)) // 第2s内左转
    {
        linear_vel = 0;
        angular_vel = 0.4;
    }
    else if (Time::Now() - time_now > 2) // 第3s结束
    {
        finished = true;
        time_now = 0;
    }
}

} // namespace behavior_controller